#include <msp430x16x.h>

#include "recover.h"
#include "../sd/sd.h"
#include "../adc/adc.h"

/*Recovery procedure for the satellite. This will be used when it first
goes into orbit to start it up, and thereafter to recover it should something go wrong.*/

//Defines power level required to power up each subsystem, so that recover() knows if it can
//afford to power up each system. The values here at the moment are completely arbitrary
#define comspower 20
#define adcpower 15
#define cam_power 25
#define cardpower 5

int i2c_init() {
//Function to initialise I2C bus for use by check_psu()

  /* Register access commented out due to RTE */

  //U0CTL = 0x25; //Sets bits 1, 2 and 5 of the USART interface to 0 to enable I2C in master mode
  //I2COA = 0x0001; //Sets the address of the MCU on the bus to 1
  return 1;
}

int check_psu() {
//Function to read powerlevel from PSU
  int powerlevel;
//code here assigns a value to power level after requesting it from the I2C bus
  powerlevel = 100 ;
  return powerlevel; //Return integer power level to the parent function
}

int powerup_coms() { // Function to switch on coms. Returns boolean 1 or 0 for successful or not successful respectively
  return 1;
}

int powerup_adc() {
  adcInit();
  return 1;
}

int powerup_camera() {
  return 1;
}

char recover() {
  int powerlevel;

  char status = 0x0; //creates a byte to hold the status of each subsystem as they are powered on

  /* Initialises ADC ports and registers */
  adcInit();

  if(!i2c_init()) { //Initialise the I2C bus so that check_psu can get a powerlevel
    return status;
  }
  status |= 0x01;
  powerlevel = check_psu(); //Call check_psu and assign the value it returns to a variable "powerlevel"

  if(powerlevel > comspower) { //Find out of there is enough power to start coms
    if(powerup_coms()) { //If there is enough power, power up coms, check if the procedure was successful
      status |= 0x02; //If coms powerup succeeded, set its status bit to 1. If not, it remains 0

    }
  } //If the power level was not high enough, the status bit of the subsystem remains 0

  powerlevel = check_psu();
  if(powerlevel > adcpower) {
    /*if(powerup_adcs()) {
	status |= 0x08;
      } function powerup_adcs does not exist */
  }

  powerlevel = check_psu();
  if(powerlevel > cam_power) {
    if(powerup_camera()) {
	status |= 0x10;
      }
  }

  return status;
}

/*
int main() {
//Code that goes in main to determine how successful recovery was
  char status = 0x000000;
  status = recover();

  return 0;
}
*/
